We first start by treating vehicles as point objects, P and Q. For the purpose of this paper, we have determined anti-clockwise rotations to be positive and clockwise rotations to be negative. We define \(\vec{u}\) and \(\vec{v}\) as the velocities of P and Q respectively:
where:
To calculate TCA of points P and Q, we find the displacement from P to Q as a function of time. Then, in order to get the time at which the displacement is minimum, we differentiate the function, equate it to 0 and solve for time. This time is the time to closest approach (TCA) of P and Q. If P and Q collide with each other, the displacement at TCA will be 0. To calculate P and Q at time t, we use the following notation:
\[\begin{equation} P(t) = \left( {\begin{array}{c} f_x(t)\\ f_y(t)\\ \end{array} } \right) \end{equation}\]where:
Similarly:
\[\begin{equation} Q(t) = \left( {\begin{array}{c} g_x(t) \\ g_y(t) \\ \end{array} } \right) \end{equation}\]where:
\(P(t)\) and \(Q(t)\) can be understood as the integrals of velocities \(\vec{u}\) and \(\vec{v}\) respectively over time t.
Additionally, we introduce adjusted TCA, a metric that measures the time P and Q avoided a collision by. To do so, we modify our calculation of P and Q to create \(\hat{P}\) and \(\hat{Q}\) that measure the delay in the behavior initiated at \(t_0\) that would have led to a crash. Note, while the metric is perhaps most useful in circumstances where \(t_0\) marks one vehicle’s “reaction” to a threatening situation, it remains valid when no behavior change is initiated at \(t_0\), which we refer to as a “null reaction”.
\[\begin{equation} \hat{P}(t) = \left( {\begin{array}{c} f_x(t) + u_0\cdot\cos(\theta_0)\cdot Z_P\\ f_y(t) + u_0\cdot\sin(\theta_0)\cdot Z_P\\ \end{array} } \right) \end{equation}\]where:
where:
Since we assume that points \(\hat{P}\) and \(\hat{Q}\) crash at time t with delayed reactions \(Z_P\) and \(Z_Q\), we set \(\hat{P}\) and \(\hat{Q}\) equal to each other to get \(Z_P\) and \(Z_Q\) as functions of time. Here, time t denotes the time it takes for points P and Q to collide from their initial starting points.
The simplest example of our model assumes constant velocity over time:
\[\begin{equation} P(t) = P_0 + t\vec{u} \end{equation}\]where \(P_0\) and \(Q_0\) are positions of points P and Q at time \(t = 0\).
Define a distance vector \(\vec{d}(t)\) between P and Q as follows:
\[\begin{equation} \begin{split} \vec{d}(t) &= P(t) - Q(t) \\ &= P_0 - Q_0 + (\vec{u} - \vec{v})t \\ &= d_0 + (\vec{u} - \vec{v})t \end{split} \end{equation}\]where \(d_0 = P_0 - Q_0\)
We calculate the TCA of P and Q by minimizing \(\vec{d}(t)\).
An animation of the scenario is as follows:
t <- 7
inc <- .1
p1 <- data.frame(x = rep(0,t/inc + 1), y = -50+10*seq(0,t, by = inc), t = seq(0,t, by = inc))
q1 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc))
library(raster)
D <- pointDistance(p1[,-3], q1[,-3], lonlat = FALSE)
library(plotly)
df <- rbind(p1,q1)
plot1 <- df %>%
plot_ly(
x = ~x,
y = ~y,
frame = ~t,
type = 'scatter',
mode = 'markers',
showlegend = F
)
plot1
The TCA of the two points in this animation is:
TCA1 <- which.min(D)
tca1 <- seq(0,t, by = inc)[TCA1]
tca1
## [1] 4.5
Finally, the adjusted TCA gives us the graph:
Zero <- data.frame(x = rep(0,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p1[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q1[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)
The area enclosed between (0, 1) and (-1, 0) is the measure of safety from collision. The line from (0, 1) to (-1, 0) depicts all possible combinations of durations that points P and Q could have delayed their reactions by in order to just collide. Hence, any point in the area enclosed by the line and the x and y axes is the combination of delayed reaction time of P and Q that would have resulted in the avoidance of a collision. Note, in the event that P and Q collide, the area would simply be a single point- (0, 0).
We will modify our calculation of \(\vec{u}\) as follows:
Adding acceleration to each component of velocity, we get:
\[\begin{equation} \vec{u}(t) = (u_0 \cos(\theta) + a_{P0}\cos(\theta)t)\hat{i} + (u_0\sin(\theta) + a_{P0}\sin(\theta)t)\hat{j} \end{equation}\]Similarly,
\[\begin{equation} \vec{v}(t) = -(v_0 \sin(\phi) + a_{Q0}\sin(\phi)t)\hat{i} + (v_0\cos(\phi) + a_{Q0}\cos(\phi)t)\hat{j} \end{equation}\]An animation of the scenario is as follows:
p2 <- data.frame(x = rep(0,t/inc + 1), y = -25+10*seq(0,t, by = inc)-seq(0,t, by = inc)^2, t = seq(0,t, by = inc))
q2 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc))
D <- pointDistance(p2[,-3], q2[,-3], lonlat = FALSE)
df <- rbind(p2,q2)
plot2 <- df %>%
plot_ly(
x = ~x,
y = ~y,
frame = ~t,
type = 'scatter',
mode = 'markers',
showlegend = F
)
plot2
The TCA of the two points in this animation is:
TCA2 <- which.min(D)
tca2 <- seq(0,t, by = inc)[TCA2]
tca2
## [1] 4
Finally, the adjusted TCA gives us the graph:
Zero <- data.frame(x = rep(0,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p2[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q2[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)
We use the steering wheel angle to determine the turn of wheels using the steering ratio. So, turn of wheels \(=\) steering wheel angle \(\times\) steering ratio.
Since turn of the wheel is the angle by which a vehicle turns at every instant, we interpret it as the derivative of a vehicle’s angle of motion.
Let turn of wheel of point P be \(W_P\). Then:
\[\begin{equation} W_P = \frac{d\theta}{dt} \end{equation}\] Since we assume that \(W_P\) is a constant, upon integrating both sides of equation (6) with respect to t, we get the angle of motion as a function of time as follows: \[\begin{equation} \theta(t) = W_P \cdot t + \theta_0 \end{equation}\]where \(\theta_0\) is the angle at which P was turned at time \(t = 0\).
Given a fixed steering wheel angle, point P travels with velocity \(\vec{u}\) in a circular motion.
An animation of the scenario is as follows:
p3 <- data.frame(x = -300/pi + 300/pi*cos(pi*seq(0,t, by = inc)/30), y = -150/pi+300/pi*sin(pi*seq(0,t,by=inc)/30), t = seq(0,t, by = inc))
q3 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc))
D <- pointDistance(p3[,-3], q3[,-3], lonlat = FALSE)
df <- rbind(p3,q3)
plot3 <- df %>%
plot_ly(
x = ~x,
y = ~y,
frame = ~t,
type = 'scatter',
mode = 'markers',
showlegend = F
)
plot3
The TCA of the two points in this animation is:
TCA3 <- which.min(D)
tca3 <- seq(0,t, by = inc)[TCA3]
tca3
## [1] 5.1
Finally, the adjusted TCA gives us the graph:
Zero <- data.frame(x = rep(-12.7936,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p3[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q3[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)
Now, we run our metric on one of the cases from the data set received from NADS:
library(R.matlab) # Reads matlab files
library(stringr) # String manipulation
library(changepoint)
# NOTE: should have the folder "MatFiles" downloaded and extracted to the location below
# Should have the files "disposition.xls" and "randomization.xlsx" in locations below
## Read in the first file (File name is 20171101161320)
m1 <- readMat(("H:\\V2VMAT\\MatFiles\\20171101161320.mat"))
part1speed <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'VDS.Veh.Speed')]]
part1time <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'Time')]]
part1heading <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'VDS.Veh.Heading')]]
part1logstream <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'SCC.LogStreams')]][,1]
df1 <- data.frame("time" = part1time,"speed" = part1speed,"heading" = part1heading, "logstream" = part1logstream)
incdf1 <- df1[which(df1$logstream != 0),names(df1) %in% c("time","speed","heading","logstream")]
incdf1$time <- incdf1$time - incdf1$time[1]
incdf1$speed <- 0.44704*incdf1$speed
##heading angle from degree to radian and set facing direction to be 0
incdf1$heading <- incdf1$heading*pi/180
incdf1$heading <- incdf1$heading - pi
#8 is time at which logstream ==3-- will be automated later
#position of point P
for (i in 1:length(incdf1$time)){
if (i!=1){
incdf1$posyp[i] <- incdf1$speed[i]/60*cos(incdf1$heading[i])+incdf1$posyp[i-1]
incdf1$posxp[i] <- incdf1$speed[i]/60*-sin(incdf1$heading[i])+incdf1$posxp[i-1]
}
else{
incdf1$posyp[i] <- incdf1$speed[i]/60*cos(incdf1$heading[i]) - 8*incdf1$speed[i]
incdf1$posxp[i] <- incdf1$speed[i]/60*-sin(incdf1$heading[i])
}
}
#position of point Q
incdf1$posxq <- (8 - incdf1$time) *15.6464
incdf1$posyq <- incdf1$time*0
p <- data.frame(x = incdf1$posxp, y = incdf1$posyp, t = incdf1$time)
q <- data.frame(x = incdf1$posxq, y = incdf1$posyq, t = incdf1$time)
library(plotly)
df <- rbind(p,q)
pl <- df %>%
plot_ly(
x = ~x,
y = ~y,
frame = ~t,
type = 'scatter',
mode = 'markers',
showlegend = F
)
pl
Zero <- data.frame(x = 0, y = 0)
DP <- pointDistance(incdf1[ , 6:5], Zero, lonlat = FALSE)/incdf1$speed[1]
DQ <- pointDistance(incdf1[ , 7:8], Zero, lonlat = FALSE)/15.6464
plot(DP,DQ)
result base on prediction
incdf1 <- df1[which(df1$logstream != 0 & df1$logstream != 3),names(df1) %in% c("time","speed","heading","logstream")]
incdf1$speed <- 0.44704*incdf1$speed
for (j in 1:length(incdf1$time)) {
if (j ==1) {
incdf1$acceleration[j] = 0
}
else{
incdf1$acceleration[j] <- (incdf1$speed[j] - incdf1$speed[j-1]) * 60
}
}
##heading angle from degree to radian and set facing direction to be 0
incdf1$heading <- incdf1$heading*pi/180
incdf1$heading <- incdf1$heading - pi
try <- cpt.meanvar(incdf1$acceleration)
incdf1 <- incdf1[attr(try, "cpts")[1]+1:nrow(incdf1),]
incdf1 <- na.omit(incdf1)
incdf1$time <- incdf1$time - incdf1$time[1]
time_end <- incdf1$time[nrow(incdf1)]+1/60
#position of point P
df2 <- data.frame(time = 0:10)
df2$posxp <- df2$time * 0
time_stop <- incdf1$speed[1]/3
for(k in 1:nrow(df2)){
if (df2$time[k] <= time_stop){
df2$posyp[k] <- -time_end*incdf1$speed[1]+ df2$time[k]*incdf1$speed[1] - 3/2*df2$time[k]^2
}
else{
df2$posyp[k] <- -time_end*incdf1$speed[1]+ time_stop*incdf1$speed[1] - 3/2*time_stop^2
}
}
#position of point Q
df2$posxq <- (time_end - df2$time) *15.6464
df2$posyq <- df2$time*0
p <- data.frame(x = df2$posxp, y = df2$posyp, t = df2$time)
q <- data.frame(x = df2$posxq, y = df2$posyq, t = df2$time)
pq <- rbind(p, q)
pq %>% plot_ly(
x = ~x,
y = ~y,
frame = ~t,
type = 'scatter',
mode = 'markers',
showlegend = F
)
for(k in 1:nrow(df2)){
if (time_end <= time_stop){
Zp <- (time_end*incdf1$speed[1]+ time_end*incdf1$speed[1] - 3/2*time_end^2)/incdf1$speed[1]
}
else{
Zp <- (time_end*incdf1$speed[1]+ time_stop*incdf1$speed[1] - 3/2*time_stop^2)/incdf1$speed[1]
}
}
Zp
## [1] 7.643927